/*
 * SubShield Firmware
 *
 * Is connected to a wifi module und offers the controldata for the aeroquad controlboard.
 *  
 */

//includes for the Wifi Module (MICROCHIP MRF24WB0MA)
extern "C" {
	#include "uip.h"
	#include <string.h>
	#include "udpapp.h"
	#include "config.h"
}
#include <WiShield.h>

//some other needed includes for i2c, spi...	
#include <Wire.h>             

//defines for the Wifi Module 
#define WIRELESS_MODE_INFRA	1
#define WIRELESS_MODE_ADHOC	2

//defines for the channel configuration
#define PPM_MAX 2000
#define PPM_MIN 1000
#define CHANNELS 6

//other
#define DEBUG; //comment this out in production mode

#define HZ50DELAY = 50000;
#define HZ10DELAY = 100000;


//IPv4 wlan config
unsigned char local_ip[]	=	{192,168,3,2};		// IP address of WiShield
unsigned char gateway_ip[]	=	{192,168,3,1};		// router or gateway IP address
unsigned char subnet_mask[]	=	{255,255,255,0};	// subnet mask for the local network
const prog_char ssid[] PROGMEM	=	{"DroneAP"};			//SSID of the wifi network ( max 32 bytes )

unsigned char security_type	=	0;								// 0 - open; 1 - WEP; 2 - WPA; 3 - WPA2
                                      
prog_uchar wep_keys[] PROGMEM = {};
// WPA/WPA2 passphrase
const prog_char security_passphrase[] PROGMEM = {""};	// max 64 characters

// WEP 128-bit keys
// sample HEX keys
/*
{	0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,	// Key 0
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	0x00,	// Key 1
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	0x00,	// Key 2
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	0x00	// Key 3
};
*/

// setup the wireless mode
// infrastructure - connect to AP
// adhoc - connect to another WiFi device
//unsigned char wireless_mode = WIRELESS_MODE_INFRA;	//WIRELESS_MODE_INFRA
unsigned char wireless_mode = WIRELESS_MODE_ADHOC;

unsigned char ssid_len;
unsigned char security_passphrase_len;

/**
 * Main Application
 */ 
 
const int ultraSensorPin = A0;
int ultraSensorValue = 0;

//Task variables
long currentTime;
long previousTime;

long loopCounter = 0;

int TxMode = 0;

int16 channels[6];
byte switchData;

boolean hover = false;

byte bdata[13];
byte odata[12];

void setup(void)
{
	Serial.begin(115200);  
	
	//Init Wifi Module
	#ifdef DEBUG
		Serial.println("Started MRF24WB0MA initialization");
	#endif
	WiFi.init();
	#ifdef DEBUG
		Serial.println("Finished MRF24WB0MA initialization");
	#endif

	//Init uC as I2C Slave
	#ifdef DEBUG
		Serial.println("Started I2C initialization");
	#endif
	Wire.begin(2);
	Wire.onRequest(requestEvent);
	Wire.onReceive(receiveEvent);
	#ifdef DEBUG
		Serial.println("Finished I2C initialization");
	#endif
	
	#ifdef DEBUG
		//Dont know, needed?
		previousTime = micros();
	#endif
	
	for(int i = 0; i < CHANNELS; i++){
		channels[i] = 1000;
	}
	
	odata[0] = 'R';
	for(int i = 1; i < 11; i++){
		odata[i] = 0;
	}
	odata[11] = 'E';
	
	//Set start throttle to zero
	channels[3] = 1000;
}

void loop()
{
	currentTime = micros();
	long deltaTime = currentTime - previousTime;

	//50 Hz loop
	if(deltaTime >= 100000){
		ultraSensorValue = analogRead(ultraSensorPin);
		odata[1] = ultraSensorValue;
		previousTime = currentTime;
		//Serial.println(ultraSensorValue, DEC);
	}
	
	//Hover Switch
	if((switchData >> 0) & 0x01){
		channels[3] = 1200;
		hover = true;	
	}else{
		channels[3] = 1000;
		hover = false;
	}
	

	WiFi.run();
}


//I2C Request Handler
void requestEvent()
{  
	Wire.write(bdata, 13);
}
//I2C Resieve Handler
void receiveEvent(int numBytes)
{
/*
	for(int i = 0; i < numBytes; i++){
		odata[i+2] = Wire.read();
	}
*/
}


/*
 *	Wifi Implementation 
 */ 

extern "C"
{
	void udpapp_init(void)
	{
		uip_ipaddr_t addr;
		struct uip_udp_conn *c;
		
		//ip of the package resiever (currently fixed, later a remote finder app or something would be cool)
		uip_ipaddr(&addr, 192,168,3, 1);
		c = uip_udp_new(&addr, HTONS(12374));
		if(c != NULL) {
			uip_udp_bind(c, HTONS(12375));
		}
	}
	
	static void send_data(void)
	{
	      
	      odata[0] = 'R';
	      
	      odata[11] = 'E';
		memcpy(uip_appdata, odata, 12);
		uip_send(uip_appdata, 12);
		/*
	  	char str[] = "Hello. Bla\n";
	  	Serial.println((int)uip_appdata, DEC);
	
		memcpy(uip_appdata, str, strlen(str));
		uip_send(uip_appdata, strlen(str));
	      */
	}                            
	
	void udpapp_appcall(void)
	{
		if(uip_newdata()) {  
		
			Serial.println("p");
		           
			unsigned int len = uip_datalen();
			char str[len];                         
			memcpy(str, uip_appdata, len);

			//Save values
			channels[0] = ((0xff&str[1])<<8)|(0xff&str[2]);//ROLL
			channels[1] = ((0xff&str[3])<<8)|(0xff&str[4]);//PITCH
			channels[2] = ((0xff&str[5])<<8)|(0xff&str[6]);//YAW
			//channels[3] = ((0xff&str[7])<<8)|(0xff&str[8]);//THROTTLE
			channels[4] = 2000;//((0xff&str[9])<<8)|(0xff&str[10]);//STABLE
			channels[5] = 2000;//((0xff&str[11])<<8)|(0xff&str[12]);//HOLD
			switchData = str[13];
			
			//Copy data, I2C interrupt values
			for(int i = 0; i < CHANNELS; i++){
				bdata[i*2] = channels[i];
				bdata[i*2+1] = channels[i]>>8;
			}
			
			bdata[12] = switchData;
		
		
			send_data();
		}   
	}
	
	void dummy_app_appcall(void){}
}
